using System;
using System.Collections;
using System.Collections.Generic;

namespace LightCAD.MathLib
{
    public class Quaternion : IEnumerable
    {
        #region Properties

        internal double _x;
        internal double _y;
        internal double _z;
        internal double _w;

        private Action OnChangeCallbackAction;

        #endregion

        #region constructor
        public Quaternion(double x = 0, double y = 0, double z = 0, double w = 1)
        {
            this._x = x;
            this._y = y;
            this._z = z;
            this._w = w;
        }
        #endregion

        #region properties
        public double X
        {
            get
            { 
                return this._x;
            }
            set
            {
                this._x = value;
                this.OnChangeCallback();
            }
        }
        public double Y
        {
            get
            {
                return this._y;
            }
            set
            {
                this._y = value;
                this.OnChangeCallback();
            }
        }
        public double Z
        {
            get
            {
                return this._z;
            }
            set
            {
                this._z = value;
                this.OnChangeCallback();
            }
        }
        public double W
        {
            get
            {
                return this._w;
            }
            set
            {
                this._w = value;
                this.OnChangeCallback();
            }
        }
        #endregion

        #region methods
        public static void SlerpFlat(IList<double> dst, int dstOffset, IList<double> src0, int srcOffset0, IList<double> src1, int srcOffset1, double t)
        {
            // fuzz-free, array-based Quaternion SLERP operation
            double x0 = src0[srcOffset0 + 0],
                y0 = src0[srcOffset0 + 1],
                z0 = src0[srcOffset0 + 2],
                w0 = src0[srcOffset0 + 3];
            double x1 = src1[srcOffset1 + 0],
                y1 = src1[srcOffset1 + 1],
                z1 = src1[srcOffset1 + 2],
                w1 = src1[srcOffset1 + 3];
            if (t == 0)
            {
                dst[dstOffset + 0] = x0;
                dst[dstOffset + 1] = y0;
                dst[dstOffset + 2] = z0;
                dst[dstOffset + 3] = w0;
                return;
            }
            if (t == 1)
            {
                dst[dstOffset + 0] = x1;
                dst[dstOffset + 1] = y1;
                dst[dstOffset + 2] = z1;
                dst[dstOffset + 3] = w1;
                return;
            }
            if (w0 != w1 || x0 != x1 || y0 != y1 || z0 != z1)
            {
                double s = 1 - t;
                double cos = x0 * x1 + y0 * y1 + z0 * z1 + w0 * w1,
                    dir = (cos >= 0 ? 1 : -1),
                    sqrSin = 1 - cos * cos;
                // Skip the Slerp for tiny steps to avoid numeric problems:
                if (sqrSin > MathEx.EPSILON)
                {
                    double sin = Math.Sqrt(sqrSin),
                        len = Math.Atan2(sin, cos * dir);
                    s = Math.Sin(s * len) / sin;
                    t = Math.Sin(t * len) / sin;
                }
                double tDir = t * dir;
                x0 = x0 * s + x1 * tDir;
                y0 = y0 * s + y1 * tDir;
                z0 = z0 * s + z1 * tDir;
                w0 = w0 * s + w1 * tDir;
                // Normalize in case we just did a lerp:
                if (s == 1 - t)
                {
                    double f = 1 / Math.Sqrt(x0 * x0 + y0 * y0 + z0 * z0 + w0 * w0);
                    x0 *= f;
                    y0 *= f;
                    z0 *= f;
                    w0 *= f;
                }
            }
            dst[dstOffset] = x0;
            dst[dstOffset + 1] = y0;
            dst[dstOffset + 2] = z0;
            dst[dstOffset + 3] = w0;
        }
        public static void MultiplyQuaternionsFlat(IList<double> dst, int dstOffset, IList<double> src0, int srcOffset0, IList<double> src1, int srcOffset1)
        {
            double x0 = src0[srcOffset0];
            double y0 = src0[srcOffset0 + 1];
            double z0 = src0[srcOffset0 + 2];
            double w0 = src0[srcOffset0 + 3];
            double x1 = src1[srcOffset1];
            double y1 = src1[srcOffset1 + 1];
            double z1 = src1[srcOffset1 + 2];
            double w1 = src1[srcOffset1 + 3];
            dst[dstOffset] = x0 * w1 + w0 * x1 + y0 * z1 - z0 * y1;
            dst[dstOffset + 1] = y0 * w1 + w0 * y1 + z0 * x1 - x0 * z1;
            dst[dstOffset + 2] = z0 * w1 + w0 * z1 + x0 * y1 - y0 * x1;
            dst[dstOffset + 3] = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1;
        }
        public Quaternion Set(double x, double y, double z, double w)
        {
            this._x = x;
            this._y = y;
            this._z = z;
            this._w = w;
            this.OnChangeCallback();
            return this;
        }
        public Quaternion Clone()
        {
            return new Quaternion(this._x, this._y, this._z, this._w);
        }
        public Quaternion Copy(Quaternion quaternion)
        {
            this._x = quaternion.X;
            this._y = quaternion.Y;
            this._z = quaternion.Z;
            this._w = quaternion.W;
            this.OnChangeCallback();
            return this;
        }
        public Quaternion SetFromEuler(Euler euler, bool update = false)
        {
            double x = euler._x, y = euler._y, z = euler._z;
            Euler.RotationOrders order = euler._order;
            // http://www.mathworks.com/matlabcentral/fileexchange/
            // 	20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
            //	content/SpinCalc.m
            double c1 = Math.Cos(x / 2);
            double c2 = Math.Cos(y / 2);
            double c3 = Math.Cos(z / 2);
            double s1 = Math.Sin(x / 2);
            double s2 = Math.Sin(y / 2);
            double s3 = Math.Sin(z / 2);
            switch (order)
            {
                case Euler.RotationOrders.XYZ:
                    this._x = s1 * c2 * c3 + c1 * s2 * s3;
                    this._y = c1 * s2 * c3 - s1 * c2 * s3;
                    this._z = c1 * c2 * s3 + s1 * s2 * c3;
                    this._w = c1 * c2 * c3 - s1 * s2 * s3;
                    break;
                case Euler.RotationOrders.YXZ:
                    this._x = s1 * c2 * c3 + c1 * s2 * s3;
                    this._y = c1 * s2 * c3 - s1 * c2 * s3;
                    this._z = c1 * c2 * s3 - s1 * s2 * c3;
                    this._w = c1 * c2 * c3 + s1 * s2 * s3;
                    break;
                case Euler.RotationOrders.ZXY:
                    this._x = s1 * c2 * c3 - c1 * s2 * s3;
                    this._y = c1 * s2 * c3 + s1 * c2 * s3;
                    this._z = c1 * c2 * s3 + s1 * s2 * c3;
                    this._w = c1 * c2 * c3 - s1 * s2 * s3;
                    break;
                case Euler.RotationOrders.ZYX:
                    this._x = s1 * c2 * c3 - c1 * s2 * s3;
                    this._y = c1 * s2 * c3 + s1 * c2 * s3;
                    this._z = c1 * c2 * s3 - s1 * s2 * c3;
                    this._w = c1 * c2 * c3 + s1 * s2 * s3;
                    break;
                case Euler.RotationOrders.YZX:
                    this._x = s1 * c2 * c3 + c1 * s2 * s3;
                    this._y = c1 * s2 * c3 + s1 * c2 * s3;
                    this._z = c1 * c2 * s3 - s1 * s2 * c3;
                    this._w = c1 * c2 * c3 - s1 * s2 * s3;
                    break;
                case Euler.RotationOrders.XZY:
                    this._x = s1 * c2 * c3 - c1 * s2 * s3;
                    this._y = c1 * s2 * c3 - s1 * c2 * s3;
                    this._z = c1 * c2 * s3 + s1 * s2 * c3;
                    this._w = c1 * c2 * c3 + s1 * s2 * s3;
                    break;
            }
            if (update != false) this.OnChangeCallback();
            return this;
        }
        public Quaternion SetFromAxisAngle(Vector3 axis, double angle)
        {
            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
            // assumes axis is normalized
            double halfAngle = angle / 2, s = Math.Sin(halfAngle);
            this._x = axis.X * s;
            this._y = axis.Y * s;
            this._z = axis.Z * s;
            this._w = Math.Cos(halfAngle);
            this.OnChangeCallback();
            return this;
        }
        public Quaternion SetFromRotationMatrix(Matrix4 m)
        {
            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
            // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
            double[] te = m.Elements;
            double m11 = te[0], m12 = te[4], m13 = te[8],
                m21 = te[1], m22 = te[5], m23 = te[9],
                m31 = te[2], m32 = te[6], m33 = te[10],
                trace = m11 + m22 + m33;
            if (trace > 0)
            {
                double s = 0.5 / Math.Sqrt(trace + 1.0);
                this._w = 0.25 / s;
                this._x = (m32 - m23) * s;
                this._y = (m13 - m31) * s;
                this._z = (m21 - m12) * s;
            }
            else if (m11 > m22 && m11 > m33)
            {
                double s = 2.0 * Math.Sqrt(1.0 + m11 - m22 - m33);
                this._w = (m32 - m23) / s;
                this._x = 0.25 * s;
                this._y = (m12 + m21) / s;
                this._z = (m13 + m31) / s;
            }
            else if (m22 > m33)
            {
                double s = 2.0 * Math.Sqrt(1.0 + m22 - m11 - m33);
                this._w = (m13 - m31) / s;
                this._x = (m12 + m21) / s;
                this._y = 0.25 * s;
                this._z = (m23 + m32) / s;
            }
            else
            {
                double s = 2.0 * Math.Sqrt(1.0 + m33 - m11 - m22);
                this._w = (m21 - m12) / s;
                this._x = (m13 + m31) / s;
                this._y = (m23 + m32) / s;
                this._z = 0.25 * s;
            }
            this.OnChangeCallback();
            return this;
        }
        public Quaternion SetFromUnitVectors(Vector3 vFrom, Vector3 vTo)
        {
            // assumes direction vectors vFrom and vTo are normalized
            double r = vFrom.Dot(vTo) + 1;
            if (r < MathEx.EPSILON)
            {
                // vFrom and vTo point in opposite directions
                r = 0;
                if (Math.Abs(vFrom.X) > Math.Abs(vFrom.Z))
                {
                    this._x = -vFrom.Y;
                    this._y = vFrom.X;
                    this._z = 0;
                    this._w = r;
                }
                else
                {
                    this._x = 0;
                    this._y = -vFrom.Z;
                    this._z = vFrom.Y;
                    this._w = r;
                }
            }
            else
            {
                // crossVectors( vFrom, vTo ); // inlined to avoid cyclic dependency on Vector3
                this._x = vFrom.Y * vTo.Z - vFrom.Z * vTo.Y;
                this._y = vFrom.Z * vTo.X - vFrom.X * vTo.Z;
                this._z = vFrom.X * vTo.Y - vFrom.Y * vTo.X;
                this._w = r;
            }
            return this.Normalize();
        }
        public double AngleTo(Quaternion q)
        {
            return 2 * Math.Acos(Math.Abs(MathEx.Clamp(this.Dot(q), -1, 1)));
        }
        public Quaternion RotateTowards(Quaternion q, double step)
        {
            double angle = this.AngleTo(q);
            if (angle == 0) return this;
            double t = Math.Min(1, step / angle);
            this.Slerp(q, t);
            return this;
        }
        public Quaternion Identity()
        {
            return this.Set(0, 0, 0, 1);
        }
        public Quaternion Invert()
        {
            // quaternion is assumed to have unit length
            return this.Conjugate();
        }
        public Quaternion Conjugate()
        {
            this._x *= -1;
            this._y *= -1;
            this._z *= -1;
            this.OnChangeCallback();
            return this;
        }
        public double Dot(Quaternion v)
        {
            return this._x * v._x + this._y * v._y + this._z * v._z + this._w * v._w;
        }
        public double LengthSq()
        {
            return this._x * this._x + this._y * this._y + this._z * this._z + this._w * this._w;
        }
        public double Length()
        {
            return Math.Sqrt(this._x * this._x + this._y * this._y + this._z * this._z + this._w * this._w);
        }
        public Quaternion Normalize()
        {
            double l = this.Length();
            if (l == 0)
            {
                this._x = 0;
                this._y = 0;
                this._z = 0;
                this._w = 1;
            }
            else
            {
                l = 1 / l;
                this._x = this._x * l;
                this._y = this._y * l;
                this._z = this._z * l;
                this._w = this._w * l;
            }
            this.OnChangeCallback();
            return this;
        }
        public Quaternion Multiply(Quaternion q)
        {
            return this.MultiplyQuaternions(this, q);
        }
        public Quaternion Premultiply(Quaternion q)
        {
            return this.MultiplyQuaternions(q, this);
        }
        public Quaternion MultiplyQuaternions(Quaternion a, Quaternion b)
        {
            // from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
            double qax = a._x, qay = a._y, qaz = a._z, qaw = a._w;
            double qbx = b._x, qby = b._y, qbz = b._z, qbw = b._w;
            this._x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
            this._y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
            this._z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
            this._w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;
            this.OnChangeCallback();
            return this;
        }
        public Quaternion Slerp(Quaternion qb, double t)
        {
            if (t == 0) return this;
            if (t == 1) return this.Copy(qb);
            double x = this._x, y = this._y, z = this._z, w = this._w;
            // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
            double cosHalfTheta = w * qb._w + x * qb._x + y * qb._y + z * qb._z;
            if (cosHalfTheta < 0)
            {
                this._w = -qb._w;
                this._x = -qb._x;
                this._y = -qb._y;
                this._z = -qb._z;
                cosHalfTheta = -cosHalfTheta;
            }
            else
            {
                this.Copy(qb);
            }
            if (cosHalfTheta >= 1.0)
            {
                this._w = w;
                this._x = x;
                this._y = y;
                this._z = z;
                return this;
            }
            double sqrSinHalfTheta = 1.0 - cosHalfTheta * cosHalfTheta;
            if (sqrSinHalfTheta <= MathEx.EPSILON)
            {
                double s = 1 - t;
                this._w = s * w + t * this._w;
                this._x = s * x + t * this._x;
                this._y = s * y + t * this._y;
                this._z = s * z + t * this._z;
                this.Normalize();
                this.OnChangeCallback();
                return this;
            }
            double sinHalfTheta = Math.Sqrt(sqrSinHalfTheta);
            double halfTheta = Math.Atan2(sinHalfTheta, cosHalfTheta);
            double ratioA = Math.Sin((1 - t) * halfTheta) / sinHalfTheta,
                ratioB = Math.Sin(t * halfTheta) / sinHalfTheta;
            this._w = (w * ratioA + this._w * ratioB);
            this._x = (x * ratioA + this._x * ratioB);
            this._y = (y * ratioA + this._y * ratioB);
            this._z = (z * ratioA + this._z * ratioB);
            this.OnChangeCallback();
            return this;
        }
        public object SlerpQuaternions(Quaternion qa, Quaternion qb, double t)
        {
            return this.Copy(qa).Slerp(qb, t);
        }
        public object Random()
        {
            // Derived from http://planning.cs.uiuc.edu/node198.html
            // Note, this source uses w, x, y, z ordering,
            // so we swap the order below.
            double u1 = MathEx.Random();
            double sqrt1u1 = Math.Sqrt(1 - u1);
            double sqrtu1 = Math.Sqrt(u1);
            double u2 = 2 * MathEx.PI * MathEx.Random();
            double u3 = 2 * MathEx.PI * MathEx.Random();
            return this.Set(
                sqrt1u1 * Math.Cos(u2),
                sqrtu1 * Math.Sin(u3),
                sqrtu1 * Math.Cos(u3),
                sqrt1u1 * Math.Sin(u2)

            );
        }
        public bool Equals(Quaternion quaternion)
        {
            return (quaternion._x == this._x) && (quaternion._y == this._y) && (quaternion._z == this._z) && (quaternion._w == this._w);
        }
        public Quaternion FromArray(double[] array, int offset = 0)
        {
            this._x = array[offset];
            this._y = array[offset + 1];
            this._z = array[offset + 2];
            this._w = array[offset + 3];
            this.OnChangeCallback();
            return this;
        }
        public double[] ToArray(double[] array = null, int offset = 0)
        {
            if (array == null) array = new double[4];
            array[offset] = this._x;
            array[offset + 1] = this._y;
            array[offset + 2] = this._z;
            array[offset + 3] = this._w;
            return array;
        }
       
        public Quaternion OnChange(Action callback)
        {
            this.OnChangeCallbackAction = callback;
            return this;
        }
        public void OnChangeCallback()
        {
            OnChangeCallbackAction?.Invoke();
        }
        #endregion

        public IEnumerator GetEnumerator()
        {
            yield return this._x;
            yield return this._y;
            yield return this._z;
            yield return this._w;
        }



    }
}
